#ifndef PathSetup_h
#define PathSetup_h

#include "Arduino.h"

#define EEPROM_MAX 0x100
#define COLS       16

class PathSetup {
  boolean doUpload;
  int     uploadIdx;
  unsigned char    upNum, upCmd, upChk, chkTmp;
  long    upLat, upLon;
public:
  PathSetup() {doUpload = false;}
  
  inline void updateCommandIO (Stream* stream, AvcUtmPath* path) {
    if (stream->available() > 0) {
      unsigned char cc = stream->read();
      if (doUpload) {
        // Process uploaded waypoint packed as $nnLaLaLaLaLnLnLnLnSSCC
        //                                                111111111122
        //                                      0123456789012345678901
        // Note: checksum is sume of hex nibbles between $ and CC byte
        //
        //  $0000000000000000000000
        //
        unsigned char hexVal = fromHex(cc);
        if (uploadIdx < 2) {              // Coordinate number (0-255)
          upNum = (upNum << 4) + hexVal;
          chkTmp += hexVal;
        } else if (uploadIdx < 10) {      // Latitude
          upLat = (upLat << 4) + hexVal;
          chkTmp += hexVal;
        } else if (uploadIdx < 18) {      // Longitude
          upLon = (upLon << 4) + hexVal;
          chkTmp += hexVal;
        } else if (uploadIdx < 20) {      // Cmd (Speed, etc)
          upCmd = (upCmd << 4) + hexVal;
          chkTmp += hexVal;
        } else if (uploadIdx < 22) {      // Checksum
          upChk = (upChk << 4) + hexVal;
        }
        uploadIdx++;
        if (uploadIdx >= 22) {
          if (upChk == chkTmp  &&  upNum == path->getNumberOfWaypointsSet()) {
            int ctrl = (int) upCmd;
            path->addWaypoint(upLat/10, upLon/10);
            path->writeToEeprom(50);
            stream->println(F("OK"));
          } else {
            stream->println(F("FAIL"));
          }
          doUpload = false;
        }
      } else {
        switch (cc) {
         case 'z':
           path->resetWaypoints();
          break;
       case '$':
          doUpload = true;
          uploadIdx = 0;
          chkTmp = 0;
          break;
        case 'w':
        case 'W':
          // Dump Current waypoints
          {
            int ways = path->getNumberOfWaypointsSet();
            if (ways > 0) {
              for (int ii = 0; ii < ways; ii++) {
                long elat, elon;
                int ctrl;
                elat = path->getLatitudeForDisplay(ii);
                elon = path->getLongitudeForDisplay(ii);
                stream->print(F("$WAY,"));
                stream->print(ii + 1, DEC);
                stream->print(F(","));
                monitorLatLon(elat, stream);
                stream->print(F(","));
                monitorLatLon(elon, stream);
                stream->print(F(","));
                stream->print(ctrl, DEC);
                stream->println();
              }
            } else {
              stream->println(F("No waypoints"));
            }
          }
          break;
  #if 0
        case 'e':
        case 'E':
          // Dump RAW EEPROM Data
          for (unsigned int lines = 0; lines < EEPROM_MAX; lines += COLS) {
            monitorHex(lines, 4);
            stream->print(F(": "));
            boolean first = true;
            for (unsigned int col = 0; col < COLS; col++) {
              if (!first)
                stream->print(F(",  "));
               first = false;
              byte val = readEepromByte(lines + col);
              monitorHex(val, 2);
            }
            stream->println();
          }
          stream->println();
          break;
  #endif
        }
      }
    }
  }

  inline unsigned char fromHex (unsigned char cc) {
    if (cc >= '0' && cc <= '9')
      return cc - '0';
    if (cc >= 'a' && cc <= 'f')
      return cc - 'a' + 10;
    if (cc >= 'A' && cc <= 'F')
      return cc - 'A' + 10;
    return 0;
  }

  // Convert 1e-7 value packed into long into decimal format
  inline void monitorLatLon (long val, Stream* stream) {
    char dBuf[14];
    PString dec(dBuf, sizeof(dBuf));
    dec.print(val, DEC);
    char len = dec.length();
    char ii = 0;
    while (ii < (len - 6)) {
      stream->write(dBuf[ii++]);
    }
    stream->write('.');
    while (ii < len) {
      stream->write(dBuf[ii++]);
    }
  }
};

#endif
